function res = trebufunV3_energy(theta,phi,psi, t,R1,R2, L1, L2,M1,M2,g)
phi = (phi/180)*pi;
theta = (theta/180) *pi;
psi = (psi/180) *pi;
trebvars = [theta;phi;psi;0;0;0] ;
options = odeset('Events', @events);
InitialPE = -M1*g*R1*sin(theta)+M1*g*R2*sin(psi)+M2*g*L1*sin(theta)+M2*g*L2*sin(phi);
%     function [value,isterminal,direction] = events(t,trebvars)
% 
%        value= mod(trebvars(1),2*pi) - mod(trebvars(2),2*pi)+pi/6;
%        value=sin(value+pi);
%         isterminal = 1;
%         direction = -1;
%     end
[T,K] = ode45(@(t,trebvars)acceleration(trebvars,L1,L2,M1,M2,R1,R2,g),[0,t],trebvars)%options);
% res = [T,K];
theta = K(:,1);
phi = K(:,2);
psi = K(:,3);
thetadot = K(:,4);
phidot = K(:,5);
psidot = K(:,6);
 for i = 1:length(theta)
 TotalE(i) = InitialPE;
 KE(i) = 1/2*M1*((R1^2)*(thetadot(i)^2)+(R2^2)*(psidot(i)^2)-2*R1*R2*thetadot(i)*psidot(i)*cos(theta(i)-psi(i)))+1/2*M2*((L1^2)*(thetadot(i)^2)+(L2^2)*(phidot(i)^2) + 2*L1*L2*thetadot(i)*phidot(i)*cos(theta(i)-phi(i)));
 PE(i) = -M1*g*R1*sin(theta(i))+M1*g*R2*sin(psi(i))+M2*g*L1*sin(theta(i))+M2*g*L2*sin(phi(i));
Energy(i) = KE(i)+PE(i);
 end
 KE(1)
 PE(1)
 clf
thetadot(1)
phidot(1)
Energy(1)
 plot(T,Energy,'b')
 hold on
 plot(T,TotalE,'r')
 plot(T,KE,'k')
  plot(T,PE,'m')
  legend('KE + PE', 'Total Energy', 'KE', 'PE')
end

function acc=acceleration(trebvars,L1,L2,M1,M2,R,R2,g)
 theta = trebvars(1);
phi = trebvars(2);
psi=trebvars(3);
thetadot = trebvars(4);
phidot = trebvars(5);
psidot = trebvars(6);
leftover = [M1*g*R*cos(theta)-M2*g*L1*cos(theta)-M2*L1*L2*(phidot^2)*sin(theta-phi)+M1*R*R2*(psidot^2)*sin(theta-psi);
           -M2*g*L2*cos(phi)+ M2*L1*L2*(thetadot^2)*sin(theta-phi);
           -M1*g*R2*cos(psi)- M1*R*R2*(thetadot^2)*sin(theta-psi)]; 
        %right side of matrix equation, top is theta
        
dotstuff = [(M1*R^2+M2*L1^2),M2*L1*L2*cos(theta-phi), -M1*R*R2*cos(theta-psi);
             L1*L2*M2*cos(theta-phi),M2*L2^2,0 ;
             R*R2*M1*cos(theta-psi),0,M1*R2^2] ;             
         %left side of matrix equation, top is theta
      
doubledot = dotstuff\leftover; 
%calulates theta and phi umblat, in that order
    

acc = [thetadot;phidot;psidot;doubledot(1);doubledot(2);doubledot(3)];
end
%200,280,50,30,4.6929,2,15.3071,4,500,10,9.8
